/*******************************************************************
 *
 * Copyright (C), 2021-2022, LS, All rights reserved.
 *
 * Author      : 
 * Create Time : 2021-10-26
 * Version     : v0.1
 * Description : 
 *******************************************************************/

/*----------------------------Head file----------------------------*/
#include "string.h"
#include "ls_ble_type.h"
#include "ls_ble_queue.h"

/*-----------------------------------------------------------------*/


uint8_t ls_ble_queue_init(ls_ble_queue_t *q, void *buf, uint8_t queue_size, uint8_t elem_size)
{
    if (buf == NULL || q == NULL)
        return LS_BLE_ERR_INVALID_PARAM;

    q->buf = buf;
    q->size = queue_size;
    q->offset = elem_size;
    q->rd_ptr = 0;
    q->wr_ptr = 0;
    q->used = 0;

    return LS_BLE_SUCCESS;
}

uint8_t ls_ble_enqueue(ls_ble_queue_t *q, void *in)
{
    if (q->used >= q->size) 
    {
        return LS_BLE_ERR_NO_MEM;
    }
	
    memcpy((uint8_t*)q->buf + q->wr_ptr * q->offset, in, q->offset);
    q->wr_ptr = (q->wr_ptr + 1) % q->size;
    q->used++;
    
    return LS_BLE_SUCCESS;
}

uint8_t ls_ble_queue_get(ls_ble_queue_t *q, void *out)
{
    if (q->used > 0) 
	{
        memcpy(out, (uint8_t*)q->buf + q->rd_ptr * q->offset, q->offset);
        return LS_BLE_SUCCESS;
    } 
	else
	{
        return LS_BLE_ERR_INVALID_STATE;
	}
}

uint8_t ls_ble_dequeue(ls_ble_queue_t *q, void *out)
{
    if (q->used > 0) 
	{
        memcpy(out, (uint8_t*)q->buf + q->rd_ptr * q->offset, q->offset);
        q->rd_ptr = (q->rd_ptr + 1) % q->size;
        q->used--;
        return LS_BLE_SUCCESS;
    } 
	else
	{
        return LS_BLE_ERR_INVALID_STATE;
	}
}

void ls_ble_queue_decrease(ls_ble_queue_t *q)
{
    if (q->used > 0) 
	{
        q->rd_ptr = (q->rd_ptr + 1) % q->size;
        q->used--;
    } 
}


void ls_ble_queue_flush(ls_ble_queue_t *q)
{
    q->rd_ptr = 0;
    q->wr_ptr = 0;
    q->used = 0;
}

uint8_t ls_ble_get_queue_used(ls_ble_queue_t *q)
{
	return q->used;
}

